/** \file */
#include "robotfinder.hpp"
#include <cmath>
#include <cstdio>
#include <iostream>
#include <algorithm>
using namespace std;

// Helper Structs
/// Holds current image, finder and mode to be passed between threads
class holder{

public:
  Mat image;
  Finder * finder;
  char mode;
};


class KeyPair{

public:
    KeyPair(){}
    KeyPair(double k, double v){
        key     = k;
        value   = v;
    }
    double key;
    double value;

    friend bool operator < (const KeyPair &, const KeyPair &);
};

bool operator < (const KeyPair & p1, const KeyPair & p2){
    return p1.key < p2.key;
}


/* Last part to implement maybe
SStot = sum((y - mean(y)).^2);
SSres = sum((y - result).^2);
R_sq = 1 - SSres / SStot;
*/

Mat polyFit(vector<KeyPair> pairs,int m){

    Mat X = Mat::ones(pairs.size(),m+1,CV_64F);
    Mat y_mat = Mat::zeros(pairs.size(),1,CV_64F);

    for (int i = 0; i < pairs.size(); i++){
        y_mat.at<double>(i,0) = pairs[i].key;
    }

    for (int i = 0; i < pairs.size(); i++){
        for (int j = 1; j <= m; j++){
            X.at<double>(i,j) = pow(pairs[i].value,j);
        }
    }
    Mat a = (X.t() * X).inv() * X.t() * y_mat;
    return a;
}


void drawRect(int boxSize, int x, int y, Mat image){
	int lpX = x - boxSize;
	int lpY = y - boxSize;

	(lpX > 0) ? : lpX = 0;
	(lpY > 0) ? : lpY = 0;

	int rpX = x + boxSize;
	int rpY = y + boxSize;	

	(rpY < image.rows) ? : rpY = image.rows -1;
	(rpX < image.cols) ? : rpX = image.cols -1;

	rectangle(image,Point(lpX, lpY),Point(rpX,rpY),Scalar(255,255,255),1,8);
	imshow("Image",image);
}




void setColors(Finder * finder, ColorThreshold thresh){
	vector<ColorThreshold> colors = finder->getRobotColorThresholds();
	int index = -1;

	if (!strcmp(thresh.label,"RED")){
		index = 0;
	}
	else if (!strcmp(thresh.label,"GREEN")){
		index = 1;
	}
	else if (!strcmp(thresh.label,"BLUE")){
		index = 2;
	}

	int slop = 2;
	colors[index].redThresh = thresh.redThresh;
	colors[index].greenThresh = thresh.greenThresh;	
	colors[index].blueThresh = thresh.blueThresh;

	colors[index].redSDev = thresh.redSDev * slop;
	colors[index].greenSDev = thresh.greenSDev  * slop;
	colors[index].blueSDev = thresh.blueSDev * slop;

	
	
	finder->setRobotColorThresholds(colors);
}


static void onMouseThresh( int event, int x, int y,int, void* hold )
{	
	holder * h = (holder *)hold; 
    	static int boxSize = 20;
	const int minBoxSize = 2;

    if (h->mode == 't'){

        if( event == EVENT_MOUSEMOVE  ){
        	drawRect(boxSize, x, y, h->image.clone());

	    }else if (event == EVENT_LBUTTONDOWN){

		    Mat image = h->image.clone();
		    int lpX = x - boxSize;
		    int lpY = y - boxSize;

		    if (lpX < 0) lpX = 0;
		    if (lpY < 0) lpY = 0;

		    int rpX = x + boxSize;
		    int rpY = y + boxSize;

		    (rpY < image.rows) ? : rpY = image.rows -1;
		    (rpX < image.cols) ? : rpX = image.cols -1;

		    // Setup for walking through image
            int cnIm  = image.channels();
            int colsIm = image.cols;

            // Setup for preping output
		    double meanR = 0;
		    double meanG = 0;
		    double meanB = 0;

		    double sDevR = 0;
		    double sDevG = 0;
		    double sDevB = 0;

            uchar * tempR;
		    uchar * tempG;
		    uchar * tempB;

            // Walk through image and threshold     
            for(int i = lpY; i < rpY; i++)
            {
                for(int j = lpX; j < rpX; j++)
                {
                    tempR = (uchar *)image.data + i*colsIm*cnIm + j*cnIm + 2;
                    tempG = (uchar *)image.data + i*colsIm*cnIm + j*cnIm + 1;
                    tempB = (uchar *)image.data + i*colsIm*cnIm + j*cnIm + 0;
                    
				    meanR += *tempR;
				    meanG += *tempG;
				    meanB += *tempB;

				
				
                    
                }
            }

		    meanR /= (rpY - lpY) * (rpX - lpX);
		    meanG /= (rpY - lpY) * (rpX - lpX);
		    meanB /= (rpY - lpY) * (rpX - lpX);

		    for(int i = lpY; i < rpY; i++)
            {
                for(int j = lpX; j < rpX; j++)
                {
                    tempR = (uchar *)image.data + i*colsIm*cnIm + j*cnIm + 2;
                    tempG = (uchar *)image.data + i*colsIm*cnIm + j*cnIm + 1;
                    tempB = (uchar *)image.data + i*colsIm*cnIm + j*cnIm + 0;
                    
				    double tR = (((double)*tempR) - meanR);
				    double tG = (((double)*tempG) - meanG);
				    double tB = (((double)*tempB) - meanB);

				    sDevR += tR * tR;
				    sDevG += tG * tG;
				    sDevB += tB * tB;
				
                    
                }
            }
		
		    sDevR = sqrt(sDevR / ((rpY - lpY) * (rpX - lpX)));
		    sDevG = sqrt(sDevG / ((rpY - lpY) * (rpX - lpX)));
		    sDevB = sqrt(sDevB / ((rpY - lpY) * (rpX - lpX)));
		
		    printf("Mean: (%f,%f,%f)\n",meanR,meanG,meanB);
		    printf("SDev: (%f,%f,%f)\n",sDevR,sDevG,sDevB);
		    //	imshow("Image",image);

		    char c = '\0';
		    printf("Use values? (y/n): \n");
		    cin >> c;
		    if (c == 'y'){
			    //FILE * fout = fopen("data.txt","a");
			    ColorThreshold thresh;
			    thresh.redThresh = meanR;
			    thresh.greenThresh = meanG;
			    thresh.blueThresh = meanB;
		
			    thresh.redSDev = sDevR;
			    thresh.greenSDev = sDevG;
			    thresh.blueSDev = sDevB;
			
			    printf("What color is this? (0 = red, 1 = green, 2 = blue)");
			    cin >> c;
			    if (c == '0'){
				    strcpy(thresh.label,"RED");
				    setColors(h->finder,thresh);
				    printf("Successfully used data\n");
			    }
			    else if (c == '1'){
				    strcpy(thresh.label,"GREEN");	
				    setColors(h->finder,thresh);	
				    printf("Successfully used data\n");
			    }
			    else if (c == '2'){
				    strcpy(thresh.label,"BLUE");
				    setColors(h->finder,thresh);
				    printf("Successfully used data\n");
			    }
			    else{
				    printf("Invalid entry! ...Ignoring data!\n");				
			    }
			    //fprintf(fout, "Mean: (%f,%f,%f)\n",meanR,meanG,meanB);
			    //fprintf(fout, "SDev: (%f,%f,%f)\n",sDevR,sDevG,sDevB);
			    //fclose(fout); 
			


		    }
		

	    }else if (event == EVENT_MBUTTONDOWN){
		    boxSize = ((boxSize > minBoxSize) ? boxSize - 1: minBoxSize);
		    drawRect(boxSize, x, y, h->image.clone());
	    }else if (event == EVENT_RBUTTONDOWN){
		    boxSize = ((boxSize > minBoxSize) ? boxSize + 1: minBoxSize);
		    drawRect(boxSize, x, y, h->image.clone());
	    }
    }
    else if(h->mode == 's' && event == EVENT_RBUTTONDOWN){
        // Allows user to save data points for calibration
        static vector<KeyPair> x_pairs;
        static vector<KeyPair> y_pairs;
        
        cout << "X = " << x << "\n"
             << "Y = " << y << "\n";
        {
            cout << "What is the Y distance (cm) from the center of the camera?"
                 << "Where behind is negative in the Y direction: ";
            double x_val;
            double y_val;
            cin >> y_val;
            cout << "What is the X distance (cm) from the center of the camera?"
                 << "Where to the right is negative in the X direction: ";
            cin >> x_val;
            cout << "Please note that it is unncessary to enter data points"
                 << " in sorted order. They will be automatically sorted.\n";

            cout << "Add to data set for calibration? (y,n)" << endl;
            char ans = 'n';
            cin >> ans;
            if (ans == 'y'){
                x_pairs.push_back(KeyPair(x_val,x));
                y_pairs.push_back(KeyPair(x_val,y));
            }
            
            cout << "Do you wish to perform the "
                 << "curve fit? (y,n)";
            cin >> ans;
            if (ans == 'y'){
                // Use third order polynomial regression
                int order = 3;
                if (x_pairs.size() > order){

                    sort(x_pairs.begin(),x_pairs.begin()+x_pairs.size());                    
                    sort(y_pairs.begin(),y_pairs.begin()+y_pairs.size());                    
                    Mat x_ = polyFit(x_pairs,order);
                    Mat y_ = polyFit(y_pairs,order);

                    for (int i = 0; i <= order; i++){
                        cout << "Curve fit values: \n";
                        cout << "X" << i << " = " << x_.at<double>(i) << endl;
                        cout << "Y" << i << " = " << y_.at<double>(i) << endl;
                    }
                
                }
                else{
                    cout << "Not enough data points. Please add more\n";
                }
                
            }
        }
    }



}


	

int main(){


  // Decide what calibration to run
  char ans = '\0';
  cout << "What calibration routine would you like to run?\n"
       << "For threshold use: \"t\"\n"
       << "To capture image use: \"c\"\n"
       << "To calibrate position use: \"s\"\n"
       << "To run edge detection use: \"e\"\n";  
  cin >> ans; 
  
  // Thresholding configuration algorithm
  if (ans == 't'){
     Finder finder;
     vector<Robot> bots = finder.getRobotPositions();
     Mat image = finder.getCurrentImage();
     vector<Mat> bins = finder.getBinaryImages();
     imshow("Image",image);
     imshow("Red",bins[0]);
     imshow("Green",bins[1]);
     imshow("Blue",bins[2]);

     holder h;
     h.image = image;
     h.finder = &finder;
     h.mode = ans;
     
        
     setMouseCallback( "Image", onMouseThresh, &h );
  
     while(true){
	bots = finder.getRobotPositions();
        image = finder.getCurrentImage();
        h.image = image.clone();
        
	    bins = finder.getBinaryImages();
        
        for (int i = 0; i < bots.size(); i++){
  
           printf("Position:(%f,%f) %s \n",bots[i].position.radius_cm, 
              bots[i].position.angle_rad,bots[i].label );
        }
     
        imshow("Image", image);
	    imshow("Red", bins[0]);
	    imshow("Green", bins[1]);
	    imshow("Blue", bins[2]);        
	    waitKey(0);
        bots.clear();
     }
  }
  else if (ans == 'c'){
     Finder finder;
     vector<Robot> bots = finder.getRobotPositions();
     Mat image = finder.getCurrentImage();
     vector<Mat> bins = finder.getBinaryImages();
     imshow("Image",image);
     imshow("Red",bins[0]);
     imshow("Green",bins[1]);
     imshow("Blue",bins[2]);
     
     while(true){
        image = finder.getCurrentImage();
        bots = finder.getRobotPositions();
	    bins = finder.getBinaryImages();
        
        for (int i = 0; i < bots.size(); i++){
  
           printf("Position:(%f,%f) %s \n",bots[i].position.radius_cm, 
              bots[i].position.angle_rad,bots[i].label );
        }
     
        imshow("Image", image);
	    imshow("Red", bins[0]);
	    imshow("Green", bins[1]);
	    imshow("Blue", bins[2]);        
	    waitKey(0);
        string val = "";
        printf("Save Image? (<name>,no)");
        cin >> val;
        if (val != "no"){
            imwrite(val,image);
        } 
        bots.clear();
     }
  
  }
  else if (ans == 's'){
    Finder finder;
     vector<Robot> bots = finder.getRobotPositions();
     Mat image = finder.getCurrentImage();
     vector<Mat> bins = finder.getBinaryImages();
     
     holder h;
     h.image = image;
     h.finder = &finder;
     h.mode = ans;
     
     imshow("Image", image);  
     setMouseCallback( "Image", onMouseThresh, &h );
  
     while(true){
        image = finder.getCurrentImage();
        h.image = image.clone();
        bots = finder.getRobotPositions();
//	bins = finder.getBinaryImages();
        
        for (int i = 0; i < bots.size(); i++){
  
           printf("Position:(%f,%f) %s \n",bots[i].position.radius_cm, 
              bots[i].position.angle_rad,bots[i].label );
        }
     
    	imshow("Image", image);
	//imshow("Red", bins[0]);
	//imshow("Green", bins[1]);
	//imshow("Blue", bins[2]);
  cout << "\nPress any key to continue\n";   
	waitKey(0);
        bots.clear();
     }
  }
  else if (ans == 'e'){
    Mat edges;
    Mat bgr;
    Mat gray;
    
    Finder finder;
     
    while(true){
      bgr = finder.getCurrentImage();	
     
      cvtColor(bgr,gray,CV_BGR2GRAY);
      Canny(gray,edges, 10.0, 60.0);	
      imshow("Image",bgr);
      imshow("Edge",edges);
      waitKey(0);
    }
     
  }
  else{
    cout << "Invalid configuration perameter...Exiting\n";
    return 0;
  }
       
	









	return 0;
}



